/*
 @file    registration.h
 @author  Rossella Petrucci, Alvise Rigo, Alberto Rubin
*/

#ifndef REGISTRATION_H_
#define REGISTRATION_H_

#ifndef DOXYGEN_SHOULD_SKIP_THIS
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/keypoints/sift_keypoint.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/search/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/features/pfh.h>
#include <pcl/features/pfhrgb.h>
#include <pcl/features/3dsc.h>
#include <pcl/features/shot_omp.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/kdtree/impl/kdtree_flann.hpp>
#include <pcl/registration/transformation_estimation_svd.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/correspondence_rejection_sample_consensus.h>
#include <pcl/common/transforms.h>
#include <pcl/surface/grid_projection.h>
#include <pcl/surface/gp3.h>
#include <pcl/surface/marching_cubes_greedy.h>
#include <pcl/surface/marching_cubes_greedy_dot.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/sac_model_line.h>

#endif

/*! \defgroup group2 Registration Group
  Methods that realize the registration of a source point cloud respect to a target point cloud.

  The problem of consistently aligning various 3D point cloud data views into a complete model
  is known as registration. Its goal is to find the relative positions and orientations
  of the separately acquired views in a global coordinate framework, such that the intersecting areas
  between them overlap perfectly. For every set of point cloud datasets acquired from different views,
  we therefore need a system that is able to align them together into a single point cloud model,
  so that subsequent processing steps such as segmentation and object reconstruction can be applied.

  @{
*/

void voxelCloud (pcl::PointCloud<pcl::PointXYZRGB>::Ptr input, pcl::PointCloud<pcl::PointXYZRGB>::Ptr output, double leafSize);

/*! Removes plane.
  \param[in] source the input point cloud.
  \param[out] segmented the resulting segmented point cloud.
  \param[in] threshold threshold for segmentation.
*/
void segmentation(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr source, pcl::PointCloud<pcl::PointXYZRGB>::Ptr segmented, float threshold);

/*! Computes key points in the input segmented point cloud.
  \param[in] pcd the input segmented point cloud.
  \param[out] out the resulting key points.
*/
void computeKeyPoints(pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcd, pcl::PointCloud<pcl::PointXYZI>::Ptr out);

/*! Extracts descriptors for given key points.
  \param[in] input point cloud to be used for descriptor extraction.
  \param[in] cloud locations where descriptors are to be extracted.
  \param[out] cloud_out features resulting descriptors.
*/
void extractFeatures(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr input, pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, pcl::PointCloud<pcl::FPFHSignature33>::Ptr cloud_out);

/*! Finds the correspondeces.
  \param[in] source source feature descriptors.
  \param[in] target target feature descriptors.
  \param[out] correspondences indices out of the target descriptors that correspond (nearest neighbor) to the source descriptors.
*/
void findCorrespondences(pcl::PointCloud<pcl::FPFHSignature33>::Ptr source, typename pcl::PointCloud<pcl::FPFHSignature33>::Ptr target, std::vector<int>& correspondences);

/*! Removes non-consistent correspondences.
  \param[in] st correspondeces source to target.
  \param[in] ts correspondeces target to source.
  \param[out] corrispondenzePtr .
  \param[out] corrispondenze .
  \param[in] sk source key points point cloud.
  \param[in] tk target key points point cloud.
  \param[in] threshold filter threshold.
*/
void filterCorrespondences(std::vector<int>& st, std::vector<int>& ts, pcl::CorrespondencesPtr corrispondenzePtr, std::vector<std::pair<unsigned, unsigned> >& corrispondenze, pcl::PointCloud<pcl::PointXYZI>::Ptr sk, pcl::PointCloud<pcl::PointXYZI>::Ptr tk, float threshold );

/*! Calculates the initial rigid transformation from filtered corresponding keypoints.
  \param[in] corrispondenzeP .
  \param[in] sk source key points point cloud.
  \param[in] tk target key points point cloud.
  \param[in] s source segmented point cloud.
  \param[out] s_out source transformed.
  \param[out] iniziale transformation matrix.
*/
void initialAlignment(pcl::CorrespondencesPtr corrispondenzeP, pcl::PointCloud<pcl::PointXYZI>::Ptr sk, pcl::PointCloud<pcl::PointXYZI>::Ptr tk, pcl::PointCloud<pcl::PointXYZRGB>::Ptr s, pcl::PointCloud<pcl::PointXYZRGB>::Ptr s_out, Eigen::Matrix4f& iniziale );

/*! Calculates the final rigid transformation using ICP over all points.
  \param[in] s_alg source transformed.
  \param[in] t target point cloud.
  \param[out] s_out source registered.
  \param[in] finale final alignment matrix.
*/
void finalAlignment(pcl::PointCloud<pcl::PointXYZRGB>::Ptr s_alg, pcl::PointCloud<pcl::PointXYZRGB>::Ptr t, pcl::PointCloud<pcl::PointXYZRGB>::Ptr s_out, Eigen::Matrix4f& finale);

/*! @} */ // end of group2

void cableDetection(pcl::PointCloud<pcl::PointXYZRGB>::Ptr area, pcl::PointCloud<pcl::PointXYZRGB>::Ptr out);

void selectCableArea(pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_registrata, pcl::PointCloud<pcl::PointXYZRGB>::Ptr area );

void transformCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr input, pcl::PointCloud<pcl::PointXYZRGB>::Ptr output);

void pioliDetection(pcl::PointCloud<pcl::PointXYZRGB>::Ptr input, pcl::PointCloud<pcl::PointXYZRGB>::Ptr output, pcl::PointCloud<pcl::PointXYZRGB>::Ptr p1, pcl::PointCloud<pcl::PointXYZRGB>::Ptr p2);

void findBegin(pcl::PointCloud<pcl::PointXYZRGB>::Ptr input);

bool finalCheck(pcl::PointCloud<pcl::PointXYZRGB>::Ptr p1, pcl::PointCloud<pcl::PointXYZRGB>::Ptr p2, pcl::PointCloud<pcl::PointXYZRGB>::Ptr cavo);

#endif /* REGISTRATION_H_ */
